/****************************************************************************\
 * @file     BR_CenterController.c
 * @author   傅帅
 * @date     2019/12/30
 * @brief    主控逻辑
 *
 ******************************************************************************
 * @note     主控任务的功能为:根据标志位进行手控/自控,周期性对电机更新控制指令
 ****************************************************************************/
#include "BR_Common.h"
#include "FreeRTOS.h"
#include "task.h"

/* 全局变量 */
TaskHandle_t BR_CenterControllerTaskHandler;
BR_Robot_t BR_gRobot;
/* 私有变量 */
static void prvManualMode(float *Vel, float *Dir, float *Omega);
static void prvAutoMode(float *Vel, float *Dir, float *Omega, BR_MotionMode_t *MotionMode, float *WheelsVel, float *WheelsDir);
static void prvSetField(void);
/* -------------------------------------------------- Public ----------------------------------------------- */
/** @brief 中控任务本体 **/
void BR_vCenterControllerTask(void)
{
    static BR_MotionMode_t MotionMode = CHASSIS_MODE;       // 单轮控制模式的启用标志 默认为CHASSIS_MODE
    static float Vel, Dir, Omega;                           // 控制方式1, 控制车体          (静态变量，在流程中不做修改则默认保持上一帧不变)
    static float WheelsVel[4], WheelsDir[4];                // 控制方式2, 独立控制Wheels    (静态变量，在流程中不做修改则默认保持上一帧不变)
    while(1)
    {
        /* 遥控器正常工作后第一帧确定红蓝场*/
        if(BR_gRobot.StateFlags.eField == UNSURE && BR_gRobot.RemoteController.KeyData != 0)
        {
            prvSetField();
        }
        /* 状态按键判断 */
        if(BR_RC_KEY1_DOWN)
            BR_gRobot.StateFlags.eControlMode = MANUAL;
        else
            BR_gRobot.StateFlags.eControlMode = AUTO;
        if(!(BR_RC_KEY2_DOWN))
            BR_gRobot.StateFlags.eControlMode = STOP;
        if(BR_RC_KEY3_DOWN)
            BR_gRobot.RemoteController.DebugMsgSendFlag = TRUE;
        else
            BR_gRobot.RemoteController.DebugMsgSendFlag = FALSE;
        
        /* 默认使用底盘模式，只有最终点逻辑切换成单轮控制模式 */
        MotionMode = CHASSIS_MODE;
        switch (BR_gRobot.StateFlags.eControlMode)
        {
            case MANUAL:                // MANUAL
                prvManualMode(&Vel, &Dir, &Omega);
                break;
            case AUTO:                  // AUTO
                prvAutoMode(&Vel, &Dir, &Omega, &MotionMode, WheelsVel, WheelsDir);
                break;
            default:                    // STOP 方向不变, 速度清零
                Vel = 0;
                Omega = 0;
                for(int i = 0; i < 4; i++) WheelsVel[i] = 0;
                break;
        }
        /* 最终控制指令输出 */
        if(MotionMode == CHASSIS_MODE)
            BR_gRobot.fMotionControl(Vel, Dir, Omega);
        else
            BR_gRobot.fWheelsControl(WheelsVel, WheelsDir);
        vTaskDelay((TickType_t)1000 / BR_CONTROL_LOOP_RATE);
    }
}

/** @brief 中控初始化 **/
void BR_vCenterControllerInit(void)
{
    BR_gRobot.ServoMotors.fServoMotorsPosSet = BR_vServoMotorsPosSet;
    BR_gRobot.DrivingMotors.fDrivingMotorsVelSet = BR_vDrivingMotorsVelSet;
    BR_gRobot.fMotionControl = BR_vChassisMotionControl;
    BR_gRobot.fWheelsControl = BR_vWheelsControl;
    /* 初始状态设置 */
    BR_gRobot.StateFlags.eControlMode = STOP;
    BR_gRobot.StateFlags.eWorkingFlow = IDLE;
    BR_gRobot.StateFlags.eField = UNSURE;
    BR_gRobot.StateFlags.LocationStatusFlag = FALSE;
}

/* -------------------------------------------------- Private ----------------------------------------------- */
/** @brief  自动模式下的逻辑 正常情况下均处于IDLE模式, 当某个键按下时，将模式切换成g_robot.current_working_flow */
static void prvAutoMode(float *Vel, float *Dir, float *Omega, BR_MotionMode_t *MotionMode, float *WheelsVel, float *WheelsDir)
{
    if (BR_RC_KEY15_DOWN)   // 停止当前工作流
    {
        BR_gRobot.StateFlags.eWorkingFlow = IDLE;
        BR_vResetPathFollowing();
        *MotionMode = CHASSIS_MODE;
    }
    switch (BR_gRobot.StateFlags.eWorkingFlow)
    {
        case IDLE:  // 舵向不变，速度清零
            *Vel = 0;
            *Omega = 0;
            for(int i = 0; i < 4; i++) WheelsVel[i] = 0;
            if (BR_RC_KEY14_DOWN)   // 开启当前设置的工作流
            {
                BR_gRobot.StateFlags.eWorkingFlow = BR_gRobot.RemoteController.NextWorkingFlow;
            }
            break;
        case FLOW1:         // 工作流1
            BR_vFollowPath(PathStru1, Vel, Dir, Omega, MotionMode, WheelsVel, WheelsDir);
            break;
        default:
            BR_gRobot.StateFlags.eWorkingFlow = IDLE;
            break;
    }
}

/** @brief  手动模式下的逻辑 根据按键执行对应逻辑 */
static void prvManualMode(float *Vel, float *Dir, float *Omega)
{
    *Vel = BR_gRobot.RemoteController.RightJoystickVel;
    *Dir = BR_gRobot.RemoteController.LeftJoystickDir;
    *Omega = 0;
    if (BR_RC_KEY5_DOWN)    // 舵向校准
    {
        BR_vServoMotorCalibration(2000);
    }
    if (BR_RC_KEY7_DOWN)    // 逻辑零位
    {
        BR_vServoMotorsGoLogicZeroPos();
    }
    if (BR_RC_KEY12_DOWN)   // 全场定位归零
    {
        BR_vLocatorSetAll(0, 0, 0);
    }    
    if (BR_RC_KEY14_DOWN)   // 手动模式自转角速度
    {
        *Omega = -BR_configMANUAL_CONTROL_MAX_OMEGA;
    }
    if (BR_RC_KEY15_DOWN)
    {
        *Omega = BR_configMANUAL_CONTROL_MAX_OMEGA;
    }
}

/** @brief  在第一帧遥控器正常工作后，由主循环进入，设置红蓝场 */
void prvSetField(void)
{
    /* 全场定位校准结束后才会为全局变量置场，并向全场定位下发设场帧 */
    if(BR_gRobot.StateFlags.LocationStatusFlag == TRUE)
    {        
    	if(BR_RC_KEY4_DOWN)
    	    BR_gRobot.StateFlags.eField = RED;
    	else
    	    BR_gRobot.StateFlags.eField = BLUE;
        BR_vLocatorSetField();
    }
}
